{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 02 无驱动的仿真\n",
    "\n",
    "在导入机器人的同时加入地面，并在无驱动的情况下运行一段时间的仿真。\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 重复加载机器人的过程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator,\n",
    "    # add for ground model\n",
    "    RigidTransform, CoulombFriction, HalfSpace,\n",
    "    # add for single trunk model\n",
    "    #GeometryFrame, Box, Sphere, GeometryInstance, MakePhongIllustrationProperties\n",
    ")\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "这里加载机器人并加入地面"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6001...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7001/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "382725214273447daa300446221da7f8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_fl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5562273bbebc498da94e267471ff7491",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_fl_to_thigh_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "fd2620266f6b4967b77113db0ac857a5",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_fl_to_knee_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a8a70a6a83c849e78dbe0c15df9639c0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_fr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "552c1c90444d40038671d98763f76d74",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_fr_to_thigh_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0c5dbd64c8934ac9a70d1d487c5b5c2d",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_fr_to_knee_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ccd4e30f991440d8a84b06af4f44eb9e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_hl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5908a2c252a341b5bc6be0b477db1a09",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_hl_to_thigh_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "86237902b77540869847fc63a03956d3",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_hl_to_knee_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0b5fff0327364cdda5d3f1fe699d16fd",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_hr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "552ad80a22b0459eb16efb1bb53e47da",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_hr_to_thigh_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "6c309d041878446dba90e9ed05e81c4c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_hr_to_knee_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-3)\n",
    "\n",
    "Parser(plant, scene_graph).AddModelFromFile(\"../models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "\n",
    "# Add a flat ground with friction\n",
    "X_BG = RigidTransform()\n",
    "surface_friction = CoulombFriction(\n",
    "        static_friction = 1.0,\n",
    "        dynamic_friction = 1.0)\n",
    "plant.RegisterCollisionGeometry(\n",
    "        plant.world_body(),      # the body for which this object is registered\n",
    "        X_BG,                    # The fixed pose of the geometry frame G in the body frame B\n",
    "        HalfSpace(),             # Defines the geometry of the object\n",
    "        \"ground_collision\",      # A name\n",
    "        surface_friction)        # Coulomb friction coefficients\n",
    "plant.RegisterVisualGeometry(\n",
    "        plant.world_body(),\n",
    "        X_BG,\n",
    "        HalfSpace(),\n",
    "        \"ground_visual\",\n",
    "        np.array([0.5,0.5,0.5,0.0]))    # Color set to be completely transparent\n",
    "\n",
    "plant.Finalize()\n",
    "assert plant.geometry_source_is_registered()\n",
    "\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f628777fbf0>"
      ]
     },
     "execution_count": 54,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "use_lcm = False\n",
    "\n",
    "target_realtime_rate = 1.0\n",
    "\n",
    "# Simulator setup\n",
    "simulator = Simulator(diagram, context)\n",
    "if use_lcm:\n",
    "    # If we're using LCM to send messages to another simulator or a real\n",
    "    # robot, we don't want Drake to slow things down, so we'll publish\n",
    "    # new messages as fast as possible\n",
    "    simulator.set_target_realtime_rate(0.0)\n",
    "else:\n",
    "    simulator.set_target_realtime_rate(target_realtime_rate)\n",
    "    \n",
    "# Fix the input port to zero.\n",
    "    plant_context = diagram.GetMutableSubsystemContext(\n",
    "        plant, simulator.get_mutable_context())\n",
    "    plant.get_actuation_input_port().FixValue(\n",
    "        plant_context, np.zeros(plant.num_actuators()))\n",
    "\n",
    "# Run the simulation a bit\n",
    "simulator.AdvanceTo(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f628774c1b0>"
      ]
     },
     "execution_count": 55,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#sim_time = 6.0\n",
    "sim_time = 0.5\n",
    "\n",
    "# Run the simulation!\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
